#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <geometry_msgs/msg/twist.hpp>

class LidarBehavior: public rclcpp::Node
{
public:
    LidarBehavior()
    : Node("lidar_behavior_class")
    {
        cmd_pub_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
        lidar_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
            "/scan",
            10,
            std::bind(&LidarBehavior::lidarCb, this,  std::placeholders::_1)
        );
    }

private:
    geometry_msgs::msg::Twist cmd_vel_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr lidar_sub_;
    int nCount_ = 0;

    void lidarCb(const sensor_msgs::msg::LaserScan::SharedPtr msg)
    {
        if (nCount_>0)
        {
            nCount_--;
            return;
        }
        int nNum = msg->ranges.size();
        int nMid = nNum / 2;
        float MidDist = msg->ranges[nMid];
        RCLCPP_INFO(get_logger(), "ranges[%d]=%0.3f", nMid, MidDist);

        if (MidDist < 1.5)
        {
            cmd_vel_.angular.z = 0.3;
            cmd_vel_.linear.x = 0.1;
        } else
        {
            cmd_vel_.linear.x = 0.1;
            cmd_vel_.angular.z = 0.0;
        }
        nCount_ = 100;

        cmd_pub_->publish(cmd_vel_);
    }

};

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<LidarBehavior>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}